#include "TBQuat.h"
#include "TBMat3x3.h"
#include "TBVec4.h"

namespace tb
{

	// ================================
	// Constructor
	// ================================

	TB_INLINE Mat4x4::Mat4x4()
	{
		Identity();
	}

	TB_INLINE Mat4x4::Mat4x4(float* a_Data)
	{
		for (int i = 0; i < 16; i++)
		{
			values[i] = a_Data[i];
		}
	}

	TB_INLINE Mat4x4::Mat4x4(
		float a_X1, float a_Y1, float a_Z1, float a_ScaleX,
		float a_X2, float a_Y2, float a_Z2, float a_ScaleY,
		float a_X3, float a_Y3, float a_Z3, float a_ScaleZ,
		float a_TranslateX, float a_TranslateY, float a_TranslateZ, float a_Corner
	)
	{
		values[X1] = a_X1;
		values[Y1] = a_Y1;
		values[Z1] = a_Z1;
		values[WX] = a_ScaleX;

		values[X2] = a_X2;
		values[Y2] = a_Y2;
		values[Z3] = a_Z2;
		values[WY] = a_ScaleY;

		values[X3] = a_X3;
		values[Y3] = a_Y3;
		values[Z3] = a_Z3;
		values[WZ] = a_ScaleZ;

		values[TX] = a_TranslateX;
		values[TY] = a_TranslateY;
		values[TZ] = a_TranslateZ;
		values[CZ] = a_Corner;
	}

	TB_INLINE Mat4x4::~Mat4x4()
	{

	}

	// ================================
	// Conversion
	// ================================

	TB_INLINE Mat4x4& Mat4x4::FromEulerAngles(float a_DegreesX, float a_DegreesY, float a_DegreesZ)
	{
		// OPTIM: One pass

		Identity();
		AddRotationX(a_DegreesX);
		AddRotationY(a_DegreesY);
		AddRotationZ(a_DegreesZ);

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::FromEulerAngles(const Vec3& a_Rotation)
	{
		// OPTIM: One pass

		Identity();
		AddRotationX(a_Rotation.x);
		AddRotationY(a_Rotation.y);
		AddRotationZ(a_Rotation.z);

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::FromOpenGLMatrix(float* a_Source)
	{
		for (unsigned int i = 0; i < 16; i++)
		{
			values[i] = a_Source[i];
		}

		return (*(tb::Mat4x4*)this);
	}

	TB_INLINE Mat4x4& Mat4x4::ToOpenGLMatrix(float* a_Target) const
	{
		for (unsigned int i = 0; i < 16; i++)
		{
			a_Target[i] = values[i];
		}

		return (*(tb::Mat4x4*)this);
	}

	TB_INLINE Mat4x4& Mat4x4::ToLeftHandedMatrix(float* a_Target) const
	{
		a_Target[ 0] = values[X1];
		a_Target[ 1] = values[Y1];
		a_Target[ 2] = values[Z1];
		a_Target[ 3] = values[WX];

		a_Target[ 4] = values[X3];
		a_Target[ 5] = values[Y3];
		a_Target[ 6] = values[Z3];
		a_Target[ 7] = values[WZ];

		a_Target[ 8] = values[X2];
		a_Target[ 9] = values[Y2];
		a_Target[10] = values[Z2];
		a_Target[11] = values[WY];

		a_Target[12] = values[TX];
		a_Target[13] = values[TY];
		a_Target[14] = values[TZ];
		a_Target[15] = values[CZ];

		return (*(tb::Mat4x4*)this);
	}

	TB_INLINE Mat4x4& Mat4x4::ToRowMajorMatrix(float* a_Target) const
	{
		a_Target[ 0] = values[ 0];
		a_Target[ 1] = values[ 4];
		a_Target[ 2] = values[ 8];
		a_Target[ 3] = values[12];

		a_Target[ 4] = values[ 1];
		a_Target[ 5] = values[ 5];
		a_Target[ 6] = values[ 9];
		a_Target[ 7] = values[13];

		a_Target[ 8] = values[ 2];
		a_Target[ 9] = values[ 6];
		a_Target[10] = values[10];
		a_Target[11] = values[14];

		a_Target[12] = values[ 3];
		a_Target[13] = values[ 7];
		a_Target[14] = values[11];
		a_Target[15] = values[15];

		return (*(tb::Mat4x4*)this);
	}

	TB_INLINE Mat3x3 Mat4x4::ToMat3x3() const
	{
		Mat3x3 result;

		result.values[Mat3x3::X1] = values[X1];
		result.values[Mat3x3::Y1] = values[Y1];
		result.values[Mat3x3::WX] = values[WX];

		result.values[Mat3x3::X2] = values[X2];
		result.values[Mat3x3::Y2] = values[Y2];
		result.values[Mat3x3::WY] = values[WY];

		result.values[Mat3x3::TX] = values[TX];
		result.values[Mat3x3::TY] = values[TY];
		result.values[Mat3x3::CZ] = values[CZ];

		return result;
	}

	TB_INLINE Quat Mat4x4::ToQuaternion() const
	{
		// TODO: Fix because resulting quaternion is wrong

		// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
		// article "Quaternion Calculus and Fast Animation".

		float fTrace = values[SX] + values[SY] + values[SZ];
		float fRoot;
		Quat result;

		if (fTrace > 0.f)
		{
			// |w| > 1/2, may as well choose w > 1/2
			fRoot = Math::Sqrt(fTrace + 1.0f);  // 2w
			result.w = 0.5f * fRoot;
			fRoot = 0.5f / fRoot;  // 1/(4w)
			result.x = (values[Z2] - values[Y3]) * fRoot;
			result.y = (values[X3] - values[Z1]) * fRoot;
			result.z = (values[Y1] - values[X2]) * fRoot;
		}
		else
		{
			// |w| <= 1/2
			static size_t s_iNext[3] = { 1, 2, 0 };
			size_t i = 0;
			if (values[SY] > values[SX])
			{
				i = 1;
			}
			if (values[SZ] > values[i * 4 + i])
			{
				i = 2;
			}
			size_t j = s_iNext[i];
			size_t k = s_iNext[j];

			fRoot = Math::Sqrt(values[i * 4 + i]-values[j * 4 + j]-values[k * 4 + k] + 1.0f);
			float* apkQuat[3] = { &result.x, &result.y, &result.z };
			*apkQuat[i] = 0.5f * fRoot;
			fRoot = 0.5f / fRoot;
			result.w    = (values[j * 4 + k] - values[k * 4 + j]) * fRoot;
			*apkQuat[j] = (values[i * 4 + j] + values[j * 4 + i]) * fRoot;
			*apkQuat[k] = (values[i * 4 + k] + values[k * 4 + i]) * fRoot;
		}

		return result;

		/*Quat result;

		float s, q[4];
		int i, j, k;
		int nxt[3] = { 1, 2, 0 };
		float tr = values[SX] + values[SY] + values[SZ];

		// check the diagonal
		if (tr > 0.f) 
		{
			s = Math::Sqrt(tr + 1.f);
			result.w = s / 2.f;
			s = 0.5f / s;

			result.x = (values[Z2] - values[Y3]) * s;
			result.y = (values[X3] - values[Z1]) * s;
			result.z = (values[Y1] - values[X2]) * s;
		} 
		else 
		{		
			// diagonal is negative
			i = 0;
			if (values[SY] > values[SX]) { i = 1; }
			if (values[SZ] > values[i * 4 + i]) { i = 2; }
			j = nxt[i];
			k = nxt[j];

			s = Math::Sqrt((values[i * 4 + i] - (values[j * 4 + j] + values[k * 4 + k])) + 1.f);
			q[i] = s * 0.5f;
			if (s != 0.0f) { s = 0.5f / s; }
			q[3] = (values[j * 4 + k] - values[k * 4 + j]) * s;
			q[j] = (values[i * 4 + j] + values[j * 4 + i]) * s;
			q[k] = (values[i * 4 + k] + values[k * 4 + i]) * s;

			result.x = q[0];
			result.y = q[1];
			result.z = q[2];
			result.w = q[3];
		}

		return result;*/

		/*float fourXSquaredMinus1 = values[X1] - values[Y2] - values[Z3];
		float fourYSquaredMinus1 = values[Y2] - values[X1] - values[Z3];
		float fourZSquaredMinus1 = values[Z3] - values[X1] - values[Y2];
		float fourWSquaredMinus1 = values[X1] + values[Y2] + values[Z3];

		int biggestIndex = 0;
		float fourBiggestSquaredMinus1 = fourWSquaredMinus1;
		if (fourXSquaredMinus1 > fourBiggestSquaredMinus1)
		{
			fourBiggestSquaredMinus1 = fourXSquaredMinus1;
			biggestIndex = 1;
		}
		if (fourYSquaredMinus1 > fourBiggestSquaredMinus1)
		{
			fourBiggestSquaredMinus1 = fourYSquaredMinus1;
			biggestIndex = 2;
		}
		if (fourZSquaredMinus1 > fourBiggestSquaredMinus1)
		{
			fourBiggestSquaredMinus1 = fourZSquaredMinus1;
			biggestIndex = 3;
		}

		float biggestVal = Math::Sqrt(fourBiggestSquaredMinus1 + 1.f) * 0.5f;
		float mult = 0.25f / biggestVal;

		Quat result;
		switch (biggestIndex)
		{

		case 0:
			{
				result.x = (values[Z2] - values[Y3]) * mult;
				result.y = (values[X3] - values[Z1]) * mult;
				result.z = (values[Y1] - values[X2]) * mult;
				result.w = biggestVal; 
				break;
			}

		case 1:
			{
				result.x = biggestVal;
				result.y = (values[Y1] + values[X2]) * mult;
				result.z = (values[X3] + values[Z1]) * mult;
				result.w = (values[Z2] - values[Y3]) * mult;
				break;
			}

		case 2:
			{
				result.x = (values[Y1] + values[X2]) * mult;
				result.y = biggestVal;
				result.z = (values[Z2] + values[Y3]) * mult;
				result.w = (values[X3] - values[Z1]) * mult;
				break;
			}

		case 3:
			{
				result.x = (values[X3] + values[Z1]) * mult;
				result.y = (values[Z2] + values[Y3]) * mult;
				result.z = biggestVal;
				result.w = (values[Y1] - values[X2]) * mult;
				break;
			}

		}

		return result;*/
	}

	TB_INLINE Mat4x4& Mat4x4::FromNormal(const Vec3& a_Normal, const Vec3& a_Up)
	{
		Vec3 hor = a_Normal.GetCrossProduct(a_Up).Normalize();
		Vec3 ver = hor.GetCrossProduct(a_Normal).Normalize();

		values[X1] = hor.x;
		values[Y1] = hor.y;
		values[Z1] = hor.z;
		values[WX] = 0.f;

		values[X2] = ver.x;
		values[Y2] = ver.y;
		values[Z2] = ver.z;
		values[WY] = 0.f;

		values[X3] = -a_Normal.x;
		values[Y3] = -a_Normal.y;
		values[Z3] = -a_Normal.z;
		values[WZ] = 0.f;

		values[TX] = 0.f;
		values[TY] = 0.f;
		values[TZ] = 0.f;
		values[CZ] = 1.f;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::Decompose(Quat& a_Quat, Vec3& a_Translation) const
	{
		a_Quat = ToQuaternion();
		a_Translation = GetTranslation();

		return (*(tb::Mat4x4*)this);
	}

	TB_INLINE Mat4x4& Mat4x4::Decompose(Vec3& a_Translation, Vec3& a_Rotation, Vec3& a_Scale) const
	{
		// translation

		a_Translation = GetTranslation();
		
		// scale

		a_Scale.x = Math::Sqrt((values[X1] * values[X1]) + (values[Y1] * values[Y1]) + (values[Z1] * values[Z1]));
		a_Scale.y = Math::Sqrt((values[X2] * values[X2]) + (values[Y2] * values[Y2]) + (values[Z2] * values[Z2]));
		a_Scale.z = Math::Sqrt((values[X3] * values[X3]) + (values[Y3] * values[Y3]) + (values[Z3] * values[Z3]));

		if (a_Scale.x == 0.f || a_Scale.y == 0.f || a_Scale.z == 0.f)
		{
			return (*(tb::Mat4x4*)this);
		}

		float sx = 1.f / a_Scale.x;
		float sy = 1.f / a_Scale.y;
		float sz = 1.f / a_Scale.z;

		// Detect negative scale with determinant and flip one arbitrary axis
		//if( determinant() < 0 ) scale.x = -scale.x;

		// Combined rotation matrix YXZ
		//
		// Cos[y]*Cos[z]+Sin[x]*Sin[y]*Sin[z]   Cos[z]*Sin[x]*Sin[y]-Cos[y]*Sin[z]  Cos[x]*Sin[y]	
		// Cos[x]*Sin[z]                        Cos[x]*Cos[z]                       -Sin[x]
		// -Cos[z]*Sin[y]+Cos[y]*Sin[x]*Sin[z]  Cos[y]*Cos[z]*Sin[x]+Sin[y]*Sin[z]  Cos[x]*Cos[y]

		a_Rotation.x = tb::Math::ArcSin(-values[Y3] / a_Scale.z);

		// Special case: Cos[x] == 0 (when Sin[x] is +/-1)
		float f = tb::Math::Abs(values[Y3] / a_Scale.z);
		if (f > 0.999f && f < 1.001f)
		{
			// Pin arbitrarily one of y or z to zero
			// Mathematical equivalent of gimbal lock
			a_Rotation.y = 0.f;

			// Now: Cos[x] = 0, Sin[x] = +/-1, Cos[y] = 1, Sin[y] = 0
			// => m[0][0] = Cos[z] and m[1][0] = Sin[z]
			a_Rotation.z = tb::Math::ArcTan2(-values[X2] / a_Scale.y, values[X1] / a_Scale.x);
		}
		// Standard case
		else
		{
			// rot.y = atan2f( c[2][0] / scale.z, c[2][2] / scale.z );
			//a_Rotation.y = tb::Math::RadToDeg(tb::Math::ArcTan2(values[X3] / a_Scale.z, values[Z3] / a_Scale.z));
			a_Rotation.y = tb::Math::RadToDeg(tb::Math::ArcTan2(values[Z1] / a_Scale.z, values[Z3] / a_Scale.z));
			// rot.z = atan2f( c[0][1] / scale.x, c[1][1] / scale.y );
			a_Rotation.z = tb::Math::RadToDeg(tb::Math::ArcTan2(values[Y1] / a_Scale.x, values[Y2] / a_Scale.y));

			int i = 0;
		}

		return (*(tb::Mat4x4*)this);
	}

	TB_INLINE Mat4x4 Mat4x4::GetSlerp(const Mat4x4& a_Other, float a_Percentage)
	{
		Quat quat_l;
		Vec3 trans_l;
		Decompose(quat_l, trans_l);

		Quat quat_r;
		Vec3 trans_r;
		a_Other.Decompose(quat_r, trans_r);

		Quat quat = quat_l.Slerp(quat_r, a_Percentage);
		Vec3 trans = trans_l.GetLerp(trans_r, a_Percentage);

		Mat4x4 result = quat.ToMat4x4(trans);

		return result;

		/*Mat3 delta = b ^ !a; // ^ = matrix product, ! = matrix transpose.  
		Vec3 axis;  
		flt deltaAngle;  
		delta.getAxisAngle(axis,deltaAngle);  
		Mat3 rm(axis,deltaAngle*alpha);  
		c = rm ^ a;*/

		/*Mat4x4 final = interp.ToMatrix();
		final.values[TX] = values[TX];
		final.values[TY] = values[TY];
		final.values[TZ] = values[TZ];

		return final;*/
	}

	TB_INLINE Mat4x4& Mat4x4::ToAxisAngle(Vec3& a_Axis, float& a_Angle)
	{
		/*if (values[X3] != -1.f && values[X3] != 1.f)
		{
			float yradians1 = -Math::Sin(values[X3]);
			float yradians2 = Math::pi - yradians1;

			float xradians1 = Math::ArcTan2(values[Y3] / Math::Cos(yradians1), values[Z3] / Math::Cos(yradians1));
			float xradians2 = Math::ArcTan2(values[Y3] / Math::Cos(yradians2), values[Z3] / Math::Cos(yradians2));

			float zradians1 = Math::ArcTan2(values[X2] / Math::Cos(yradians1), values[X1] / Math::Cos(yradians1));
			float zradians2 = Math::ArcTan2(values[X2] / Math::Cos(yradians2), values[X1] / Math::Cos(yradians2));

			tb::Mat4x4 test1;
			test1.AddRotationX(Math::RadToDeg(xradians1));
			test1.AddRotationY(Math::RadToDeg(yradians1));
			test1.AddRotationZ(Math::RadToDeg(zradians1));

			tb::Mat4x4 test2;
			test2.AddRotationX(Math::RadToDeg(xradians2));
			test2.AddRotationY(Math::RadToDeg(yradians2));
			test2.AddRotationZ(Math::RadToDeg(zradians2));

			int bleh = 0;
		}
		else
		{
			float zradians = 0.f;

			if (values[X3] == -1.f)
			{
				float yradians = Math::pi_half;
				float xradians = Math::ArcTan2(values[Y1], values[Z1]);
			}
			else
			{
				float yradians = -Math::pi_half;
				float xradians = Math::ArcTan2(-values[Y1], -values[Z1]);
			}

			int bleh = 0;
		}

		return *this;*/

		float epsilon = 0.01f;
		float epsilon2 = 0.1f;

		if (
			Math::Abs(values[Y1] - values[X2]) < epsilon &&
			Math::Abs(values[Z1] - values[X3]) < epsilon &&
			Math::Abs(values[Z2] - values[Y3]) < epsilon
		)
		{
			if (
				Math::Abs(values[Y1] - values[X2]) < epsilon2 &&
				Math::Abs(values[Z1] - values[X3]) < epsilon2 &&
				Math::Abs(values[Z2] - values[Y3]) < epsilon2
			)
			{
				a_Axis.x = a_Axis.y = a_Axis.z = 0.f;
				a_Angle = Math::RadToDeg(0.f);

				return *this;
			}
			
			a_Angle = Math::RadToDeg(Math::pi);
			float xx = (values[SX] + 1.f) / 2.f;
			float yy = (values[SY] + 1.f) / 2.f;
			float zz = (values[SZ] + 1.f) / 2.f;
			float xy = (values[Y1] + values[X2]) / 4.f;
			float xz = (values[Z1] + values[X3]) / 4.f;
			float yz = (values[Z2] + values[Y3]) / 4.f;

			// m[0][0] is the largest diagonal term
			if ((xx > yy) && (xx > zz)) 
			{
				if (xx < epsilon) 
				{
					a_Axis.x = 0.f;
					a_Axis.y = 0.7071f;
					a_Axis.z = 0.7071f;
				} 
				else 
				{
					a_Axis.x = Math::Sqrt(xx);
					a_Axis.y = xy / a_Axis.x;
					a_Axis.z = xz / a_Axis.x;
				}
			} 
			// m[1][1] is the largest diagonal term
			else if (yy > zz) 
			{ 
				if (yy < epsilon) 
				{
					a_Axis.x = 0.7071f;
					a_Axis.y = 0.f;
					a_Axis.z = 0.7071f;
				} 
				else 
				{
					a_Axis.y = Math::Sqrt(yy);
					a_Axis.x = xy / a_Axis.y;
					a_Axis.z = yz / a_Axis.y;
				}	
			}
			// m[2][2] is the largest diagonal term so base result on this
			else 
			{
				if (zz < epsilon) 
				{
					a_Axis.x = 0.7071f;
					a_Axis.y = 0.7071f;
					a_Axis.z = 0.f;
				} 
				else 
				{
					a_Axis.z = Math::Sqrt(zz);
					a_Axis.x = xz / a_Axis.z;
					a_Axis.y = yz / a_Axis.z;
				}
			}

			return *this;
		}

		// as we have reached here there are no singularities so we can handle normally
		/*float s = Math::Sqrt(
			(values[Y3] - values[Z2]) * (values[Y3] - values[Z2]) +
			(values[Z1] - values[X3]) * (values[Z1] - values[X3]) +
			(values[X2] - values[Y1]) * (values[X2] - values[Y1])
		); // used to normalise*/
		float s = Math::Sqrt(
			(values[Z2] - values[Y3]) * (values[Z2] - values[Y3]) +
			(values[X3] - values[Z1]) * (values[X3] - values[Z1]) +
			(values[Y1] - values[X2]) * (values[Y1] - values[X2])
		); // used to normalise

		/*			double s = Math.sqrt((m[2][1] - m[1][2])*(m[2][1] - m[1][2])
		+(m[0][2] - m[2][0])*(m[0][2] - m[2][0])
		+(m[1][0] - m[0][1])*(m[1][0] - m[0][1])); // used to normalise
		*/
		if (Math::Abs(s) < 0.001f) { s = 1.f; } 

		// prevent divide by zero, should not happen if matrix is orthogonal and should be
		// caught by singularity test above, but I've left it in just in case
		a_Angle = Math::RadToDeg(Math::ArcCos((values[SX] + values[SY] + values[SZ] - 1.f) / 2.f));
		/*a_Axis.x = (values[Y3] - values[Z2]) / s;
		a_Axis.y = (values[Y1] - values[X3]) / s;
		a_Axis.z = (values[X2] - values[Y1]) / s;*/
		a_Axis.x = (values[Z2] - values[Y3]) / s;
		a_Axis.y = (values[X3] - values[Y1]) / s;
		a_Axis.z = (values[Y1] - values[X2]) / s;

		return *this;

		/*public axisAngle toAxisAngle(matrix m) {
			double angle,x,y,z; // variables for result
			double epsilon = 0.01; // margin to allow for rounding errors
			double epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees
			// optional check that input is pure rotation, 'isRotationMatrix' is defined at:
			// http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/
			assert isRotationMatrix(m) : "not valid rotation matrix" ;// for debugging
			if ((Math.abs(m[0][1]-m[1][0])< epsilon)
				&& (Math.abs(m[0][2]-m[2][0])< epsilon)
				&& (Math.abs(m[1][2]-m[2][1])< epsilon)) {
					// singularity found
					// first check for identity matrix which must have +1 for all terms
					//  in leading diagonaland zero in other terms
					if ((Math.abs(m[0][1]+m[1][0]) < epsilon2)
						&& (Math.abs(m[0][2]+m[2][0]) < epsilon2)
						&& (Math.abs(m[1][2]+m[2][1]) < epsilon2)
						&& (Math.abs(m[0][0]+m[1][1]+m[2][2]-3) < epsilon2)) {
							// this singularity is identity matrix so angle = 0
							return new axisAngle(0,1,0,0); // zero angle, arbitrary axis
					}
					// otherwise this singularity is angle = 180
					angle = Math.PI;
					double xx = (m[0][0]+1)/2;
					double yy = (m[1][1]+1)/2;
					double zz = (m[2][2]+1)/2;
					double xy = (m[0][1]+m[1][0])/4;
					double xz = (m[0][2]+m[2][0])/4;
					double yz = (m[1][2]+m[2][1])/4;
					if ((xx > yy) && (xx > zz)) { // m[0][0] is the largest diagonal term
						if (xx< epsilon) {
							x = 0;
							y = 0.7071;
							z = 0.7071;
						} else {
							x = Math.sqrt(xx);
							y = xy/x;
							z = xz/x;
						}
					} else if (yy > zz) { // m[1][1] is the largest diagonal term
						if (yy< epsilon) {
							x = 0.7071;
							y = 0;
							z = 0.7071;
						} else {
							y = Math.sqrt(yy);
							x = xy/y;
							z = yz/y;
						}	
					} else { // m[2][2] is the largest diagonal term so base result on this
						if (zz< epsilon) {
							x = 0.7071;
							y = 0.7071;
							z = 0;
						} else {
							z = Math.sqrt(zz);
							x = xz/z;
							y = yz/z;
						}
					}
					return new axisAngle(angle,x,y,z); // return 180 deg rotation
			}
			// as we have reached here there are no singularities so we can handle normally
			double s = Math.sqrt((m[2][1] - m[1][2])*(m[2][1] - m[1][2])
				+(m[0][2] - m[2][0])*(m[0][2] - m[2][0])
				+(m[1][0] - m[0][1])*(m[1][0] - m[0][1])); // used to normalise
			if (Math.abs(s) < 0.001) s=1; 
			// prevent divide by zero, should not happen if matrix is orthogonal and should be
			// caught by singularity test above, but I've left it in just in case
			angle = Math.acos(( m[0][0] + m[1][1] + m[2][2] - 1)/2);
			x = (m[2][1] - m[1][2])/s;
			y = (m[0][2] - m[2][0])/s;
			z = (m[1][0] - m[0][1])/s;
			return new axisAngle(angle,x,y,z);*/
	}

	// ================================
	// Hooks
	// ================================

	TB_INLINE float Mat4x4::GetDeterminant() const
	{
		return 
			values[WX] * values[Z2] * values[Y3] * values[TX] - 
			values[Z1] * values[WY] * values[Y3] * values[TX] - 
			values[WX] * values[Y2] * values[Z3] * values[TX] + 
			values[Y1] * values[WY] * values[Z3] * values[TX] +
			values[Z1] * values[Y2] * values[WZ] * values[TX] - 
			values[Y1] * values[Z2] * values[WZ] * values[TX] - 
			values[WX] * values[Z2] * values[X3] * values[TY] + 
			values[Z1] * values[WY] * values[X3] * values[TY] +
			values[WX] * values[X2] * values[Z3] * values[TY] - 
			values[X1] * values[WY] * values[Z3] * values[TY] - 
			values[Z1] * values[X2] * values[WZ] * values[TY] + 
			values[X1] * values[Z2] * values[WZ] * values[TY] +
			values[WX] * values[Y2] * values[X3] * values[TZ] - 
			values[Y1] * values[WY] * values[X3] * values[TZ] - 
			values[WX] * values[X2] * values[Y3] * values[TZ] + 
			values[X1] * values[WY] * values[Y3] * values[TZ] +
			values[Y1] * values[X2] * values[WZ] * values[TZ] - 
			values[X1] * values[Y2] * values[WZ] * values[TZ] - 
			values[Z1] * values[Y2] * values[X3] * values[CZ] + 
			values[Y1] * values[Z2] * values[X3] * values[CZ] +
			values[Z1] * values[X2] * values[Y3] * values[CZ] - 
			values[X1] * values[Z2] * values[Y3] * values[CZ] - 
			values[Y1] * values[X2] * values[Z3] * values[CZ] + 
			values[X1] * values[Y2] * values[Z3] * values[CZ];
	}

	TB_INLINE Quat Mat4x4::GetRotation() const
	{
		return ToQuaternion();
	}

	TB_INLINE Vec3 Mat4x4::GetTranslation() const
	{
		return Vec3(values[TX], values[TY], values[TZ]);
	}

	TB_INLINE Vec3 Mat4x4::GetScale() const
	{
		Vec3 scale (
			Math::Sqrt(values[X1] * values[X1] + values[Y1] * values[Y1] + values[Z1] * values[Z1]),
			Math::Sqrt(values[X2] * values[X2] + values[Y2] * values[Y2] + values[Z2] * values[Z2]),
			Math::Sqrt(values[X3] * values[X3] + values[Y3] * values[Y3] + values[Z3] * values[Z3])
		);

		return scale;
	}

	// ================================
	// Assignment & Access
	// ================================

	TB_INLINE Mat4x4& Mat4x4::operator = (const float* a_Values)
	{
		for (int i = 0; i < 16; i++)
		{
			values[i] = a_Values[i];
		}

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::operator = (const Mat4x4& a_Other)
	{
		for (int i = 0; i < 16; i++)
		{
			values[i] = a_Other.values[i];
		}

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::operator = (const Mat3x3& a_Other)
	{
		values[X1] = a_Other.values[Mat3x3::X1];
		values[Y1] = a_Other.values[Mat3x3::Y1];
		values[Z1] = 0.f;
		values[WX] = a_Other.values[Mat3x3::WX];

		values[X2] = a_Other.values[Mat3x3::X2];
		values[Y2] = a_Other.values[Mat3x3::Y2];
		values[Z2] = 0.f;
		values[WY] = a_Other.values[Mat3x3::WY];

		values[X3] = 0.f;
		values[Y3] = 0.f;
		values[Z3] = 0.f;
		values[WZ] = 0.f;

		values[TX] = a_Other.values[Mat3x3::TX];
		values[TY] = a_Other.values[Mat3x3::TY];
		values[TZ] = 0.f;
		values[CZ] = a_Other.values[Mat3x3::CZ];

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::operator = (const Quat& a_Other)
	{
		*this = a_Other.ToMat4x4();

		return *this;
	}

	TB_INLINE float& Mat4x4::operator [] (int a_Index)
	{
		return (values[a_Index]);
	}

	TB_INLINE Mat4x4::operator float* ()
	{
		return values;
	}

	// ================================
	// Operations
	// ================================

	TB_INLINE Mat4x4 Mat4x4::operator + (const Mat4x4& a_Other) const
	{
		Mat4x4 result;

		for (int i = 0; i < 16; i++)
		{
			result.values[i] = values[i] + a_Other.values[i];
		}

		return result;
	}

	TB_INLINE Mat4x4& Mat4x4::operator += (const Mat4x4& a_Other)
	{
		for (int i = 0; i < 16; i++)
		{
			values[i] += a_Other.values[i];
		}

		return *this;
	}

	TB_INLINE Mat4x4 Mat4x4::operator * (const Mat4x4& a_Other) const
	{
		float a0, a1, a2, a3;

		Mat4x4 result;

		a0 = a_Other.values[X1];
		a1 = a_Other.values[Y1];
		a2 = a_Other.values[Z1];
		a3 = a_Other.values[WX];

		result.values[X1] = (a0 * values[X1]) + (a1 * values[X2]) + (a2 * values[X3]) + (a3 * values[TX]);
		result.values[Y1] = (a0 * values[Y1]) + (a1 * values[Y2]) + (a2 * values[Y3]) + (a3 * values[TY]);
		result.values[Z1] = (a0 * values[Z1]) + (a1 * values[Z2]) + (a2 * values[Z3]) + (a3 * values[TZ]);
		result.values[WX] = (a0 * values[WX]) + (a1 * values[WY]) + (a2 * values[WZ]) + (a3 * values[CZ]);

		a0 = a_Other.values[X2];
		a1 = a_Other.values[Y2];
		a2 = a_Other.values[Z2];
		a3 = a_Other.values[WY];

		result.values[X2] = (a0 * values[X1]) + (a1 * values[X2]) + (a2 * values[X3]) + (a3 * values[TX]);
		result.values[Y2] = (a0 * values[Y1]) + (a1 * values[Y2]) + (a2 * values[Y3]) + (a3 * values[TY]);
		result.values[Z2] = (a0 * values[Z1]) + (a1 * values[Z2]) + (a2 * values[Z3]) + (a3 * values[TZ]);
		result.values[WY] = (a0 * values[WX]) + (a1 * values[WY]) + (a2 * values[WZ]) + (a3 * values[CZ]);

		a0 = a_Other.values[X3];
		a1 = a_Other.values[Y3];
		a2 = a_Other.values[Z3];
		a3 = a_Other.values[WZ];

		result.values[X3] = (a0 * values[X1]) + (a1 * values[X2]) + (a2 * values[X3]) + (a3 * values[TX]);
		result.values[Y3] = (a0 * values[Y1]) + (a1 * values[Y2]) + (a2 * values[Y3]) + (a3 * values[TY]);
		result.values[Z3] = (a0 * values[Z1]) + (a1 * values[Z2]) + (a2 * values[Z3]) + (a3 * values[TZ]);
		result.values[WZ] = (a0 * values[WX]) + (a1 * values[WY]) + (a2 * values[WZ]) + (a3 * values[CZ]);

		a0 = a_Other.values[TX];
		a1 = a_Other.values[TY];
		a2 = a_Other.values[TZ];
		a3 = a_Other.values[CZ];

		result.values[TX] = (a0 * values[X1]) + (a1 * values[X2]) + (a2 * values[X3]) + (a3 * values[TX]);
		result.values[TY] = (a0 * values[Y1]) + (a1 * values[Y2]) + (a2 * values[Y3]) + (a3 * values[TY]);
		result.values[TZ] = (a0 * values[Z1]) + (a1 * values[Z2]) + (a2 * values[Z3]) + (a3 * values[TZ]);
		result.values[CZ] = (a0 * values[WX]) + (a1 * values[WY]) + (a2 * values[WZ]) + (a3 * values[CZ]);

		return result;
	}

	TB_INLINE Vec3 Mat4x4::operator * (const Vec3& a_Vector) const
	{
		Vec3 result;

		float x = (a_Vector.x * values[X1]) + (a_Vector.y * values[X2]) + (a_Vector.z * values[X3]) + values[TX];
		float y = (a_Vector.x * values[Y1]) + (a_Vector.y * values[Y2]) + (a_Vector.z * values[Y3]) + values[TY];
		float z = (a_Vector.x * values[Z1]) + (a_Vector.y * values[Z2]) + (a_Vector.z * values[Z3]) + values[TZ];
		float w = (a_Vector.x * values[WX]) + (a_Vector.y * values[WY]) + (a_Vector.z * values[WZ]) + values[CZ];

		result.x = x / w;
		result.y = y / w;
		result.z = z / w;

		return result;
	}

	TB_INLINE Vec4 Mat4x4::operator * (const Vec4& a_Vector) const
	{
		Vec4 result;

		result.x = (a_Vector.x * values[X1]) + (a_Vector.y * values[X2]) + (a_Vector.z * values[X3]) + (a_Vector.w * values[TX]);
		result.y = (a_Vector.x * values[Y1]) + (a_Vector.y * values[Y2]) + (a_Vector.z * values[Y3]) + (a_Vector.w * values[TY]);
		result.z = (a_Vector.x * values[Z1]) + (a_Vector.y * values[Z2]) + (a_Vector.z * values[Z3]) + (a_Vector.w * values[TZ]);
		result.w = (a_Vector.x * values[WX]) + (a_Vector.y * values[WY]) + (a_Vector.z * values[WZ]) + (a_Vector.w * values[CZ]);

		return result;
	}

	TB_INLINE Mat4x4 Mat4x4::operator * (float a_Value) const
	{
		Mat4x4 result;

		for (int i = 0; i < 16; i++)
		{
			result.values[i] = values[i] * a_Value;
		}

		return result;
	}

	TB_INLINE Mat4x4& Mat4x4::operator *= (const Mat4x4& a_Other)
	{
		float a0, a1, a2, a3;

		Mat4x4 temp = *this;

		a0 = a_Other.values[X1];
		a1 = a_Other.values[Y1];
		a2 = a_Other.values[Z1];
		a3 = a_Other.values[WX];

		values[X1] = (a0 * temp.values[X1]) + (a1 * temp.values[X2]) + (a2 * temp.values[X3]) + (a3 * temp.values[TX]);
		values[Y1] = (a0 * temp.values[Y1]) + (a1 * temp.values[Y2]) + (a2 * temp.values[Y3]) + (a3 * temp.values[TY]);
		values[Z1] = (a0 * temp.values[Z1]) + (a1 * temp.values[Z2]) + (a2 * temp.values[Z3]) + (a3 * temp.values[TZ]);
		values[WX] = (a0 * temp.values[WX]) + (a1 * temp.values[WY]) + (a2 * temp.values[WZ]) + (a3 * temp.values[CZ]);

		a0 = a_Other.values[X2];
		a1 = a_Other.values[Y2];
		a2 = a_Other.values[Z2];
		a3 = a_Other.values[WY];

		values[X2] = (a0 * temp.values[X1]) + (a1 * temp.values[X2]) + (a2 * temp.values[X3]) + (a3 * temp.values[TX]);
		values[Y2] = (a0 * temp.values[Y1]) + (a1 * temp.values[Y2]) + (a2 * temp.values[Y3]) + (a3 * temp.values[TY]);
		values[Z2] = (a0 * temp.values[Z1]) + (a1 * temp.values[Z2]) + (a2 * temp.values[Z3]) + (a3 * temp.values[TZ]);
		values[WY] = (a0 * temp.values[WX]) + (a1 * temp.values[WY]) + (a2 * temp.values[WZ]) + (a3 * temp.values[CZ]);

		a0 = a_Other.values[X3];
		a1 = a_Other.values[Y3];
		a2 = a_Other.values[Z3];
		a3 = a_Other.values[WZ];

		values[X3] = (a0 * temp.values[X1]) + (a1 * temp.values[X2]) + (a2 * temp.values[X3]) + (a3 * temp.values[TX]);
		values[Y3] = (a0 * temp.values[Y1]) + (a1 * temp.values[Y2]) + (a2 * temp.values[Y3]) + (a3 * temp.values[TY]);
		values[Z3] = (a0 * temp.values[Z1]) + (a1 * temp.values[Z2]) + (a2 * temp.values[Z3]) + (a3 * temp.values[TZ]);
		values[WZ] = (a0 * temp.values[WX]) + (a1 * temp.values[WY]) + (a2 * temp.values[WZ]) + (a3 * temp.values[CZ]);

		a0 = a_Other.values[TX];
		a1 = a_Other.values[TY];
		a2 = a_Other.values[TZ];
		a3 = a_Other.values[CZ];

		values[TX] = (a0 * temp.values[X1]) + (a1 * temp.values[X2]) + (a2 * temp.values[X3]) + (a3 * temp.values[TX]);
		values[TY] = (a0 * temp.values[Y1]) + (a1 * temp.values[Y2]) + (a2 * temp.values[Y3]) + (a3 * temp.values[TY]);
		values[TZ] = (a0 * temp.values[Z1]) + (a1 * temp.values[Z2]) + (a2 * temp.values[Z3]) + (a3 * temp.values[TZ]);
		values[CZ] = (a0 * temp.values[WX]) + (a1 * temp.values[WY]) + (a2 * temp.values[WZ]) + (a3 * temp.values[CZ]);

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::operator *= (float a_Value)
	{
		for (int i = 0; i < 16; i++)
		{
			values[i] *= a_Value;
		}

		return *this;
	}

	TB_INLINE Mat4x4 Mat4x4::operator / (float a_Value) const
	{
		Mat4x4 result;

		for (int i = 0; i < 16; i++)
		{
			result.values[i] = values[i] / a_Value;
		}

		return result;
	}

	TB_INLINE Mat4x4& Mat4x4::operator /= (float a_Value)
	{
		for (int i = 0; i < 16; i++)
		{
			values[i] /= a_Value;
		}

		return *this;
	}

	// ================================
	// Comparison
	// ================================

	TB_INLINE bool Mat4x4::operator == (const Mat4x4& a_Other) const
	{
		for (int i = 0; i < 16; i++)
		{
			if (values[i] != a_Other.values[i]) { return false; }
		}

		return true;
	}

	TB_INLINE bool Mat4x4::operator == (float* a_Values) const
	{
		for (int i = 0; i < 16; i++)
		{
			if (values[i] != a_Values[i]) 
			{ 
				return false; 
			}
		}

		return true;
	}

	TB_INLINE bool Mat4x4::operator != (const Mat4x4& a_Other) const
	{
		for (int i = 0; i < 16; i++)
		{
			if (values[i] != a_Other.values[i]) { return true; }
		}

		return false;
	}

	TB_INLINE bool Mat4x4::Compare(const Mat4x4& a_Other, float a_Delta) const
	{
		for (int i = 0; i < 16; i++)
		{
			if ((values[i] - a_Other.values[i]) * 2 > a_Delta)
			{
				return false; 
			}
		}

		return true;
	}

	TB_INLINE bool Mat4x4::Compare(float* a_Values, float a_Delta) const
	{
		for (int i = 0; i < 16; i++)
		{
			if ((values[i] - a_Values[i]) * 2 > a_Delta)
			{
				return false; 
			}
		}

		return true;
	}

	TB_INLINE bool Mat4x4::IsIdentity() const
	{
		float important[4] = {
			values[X1],
			values[Y2],
			values[Z3],
			values[CZ]
		};

		for (int i = 0; i < 4; i++)
		{
			if (important[i] <= 0.9999f || important[i] >= 1.0001f) { return false; }
		}

		float notsoimportant[12] = {
			/*       */ values[Y1], values[Z1], values[WX],
			values[X2], /*       */ values[Z2], values[WY],
			values[X3], values[Y3], /*       */ values[WZ],
			values[TX], values[TY], values[TZ]  /*       */
		};

		for (int i = 0; i < 12; i++)
		{
			if (notsoimportant[i] <= -0.0001f || notsoimportant[i] >= 0.0001f) { return false; }
		}

		return true;
	}

	// ================================
	// Functions
	// ================================

	TB_INLINE void Mat4x4::Clear()
	{
		for (int i = 0; i < 16; i++)
		{
			values[i] = 0.f;
		}
	}

	TB_INLINE Mat4x4& Mat4x4::Identity()
	{
		/*     */    values[Y1] = values[Z1] = values[WX] =
		values[X2] = /*	     */   values[Z2] = values[WY] =
		values[X3] = values[Y3]   /*      */ = values[WZ] =
		values[TX] = values[TY] = values[TZ]   /*      */ = 0.f;

		values[SX] = /*      */   /*      */   /*      */
		/*      */   values[SY] = /*      */   /*      */
		/*      */   /*      */   values[SZ] = /*      */
		/*      */   /*      */   /*      */   values[CZ] = 1.f;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::Concatenate(const Mat4x4& a_Other)
	{
		// TODO: Unroll loops

		Mat4x4 result;

		for (int cell = 0; cell < 4; cell++) 
		{
			for (int row = 0; row < 4; row++)
			{
				result.values[row * 4 + cell] = 
					values[row * 4 + 0] * a_Other.values[cell +  0] +
					values[row * 4 + 1] * a_Other.values[cell +  4] +
					values[row * 4 + 2] * a_Other.values[cell +  8] +
					values[row * 4 + 3] * a_Other.values[cell + 12];
			}	
		}

		for (int i = 0; i < 16; i++)
		{
			values[i] = result.values[i];
		}

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::SetScale(float a_Scale)
	{
		Vec3 scale (
			Math::InvSqrt(values[X1] * values[X1] + values[Y1] * values[Y1] + values[Z1] * values[Z1]),
			Math::InvSqrt(values[X2] * values[X2] + values[Y2] * values[Y2] + values[Z2] * values[Z2]),
			Math::InvSqrt(values[X3] * values[X3] + values[Y3] * values[Y3] + values[Z3] * values[Z3])
		);

		values[SX] = values[SX] * scale.x * a_Scale;
		values[SY] = values[SY] * scale.y * a_Scale;
		values[SZ] = values[SZ] * scale.z * a_Scale;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::SetScale(const Vec3& a_Scale)
	{
		Vec3 scale (
			Math::InvSqrt(values[X1] * values[X1] + values[Y1] * values[Y1] + values[Z1] * values[Z1]),
			Math::InvSqrt(values[X2] * values[X2] + values[Y2] * values[Y2] + values[Z2] * values[Z2]),
			Math::InvSqrt(values[X3] * values[X3] + values[Y3] * values[Y3] + values[Z3] * values[Z3])
		);

		values[SX] = values[SX] * scale.x * a_Scale.x;
		values[SY] = values[SY] * scale.y * a_Scale.y;
		values[SZ] = values[SZ] * scale.z * a_Scale.z;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::AddScale(float a_Scale)
	{
		values[SX] += a_Scale;
		values[SY] += a_Scale;
		values[SZ] += a_Scale;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::AddScale(const Vec3& a_Scale)
	{
		values[SX] += a_Scale.x;
		values[SY] += a_Scale.y;
		values[SZ] += a_Scale.z;

		return *this;
	}

	// ================================
	// Translation
	// ================================

	TB_INLINE Mat4x4& Mat4x4::SetTranslationX(float a_X)
	{
		/*values[TX] = (a_X * values[X1]);
		values[TY] = (a_X * values[Y1]);
		values[TZ] = (a_X * values[Z1]);*/

		values[TX] = a_X;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::SetTranslationY(float a_Y)
	{
		/*values[TX] = (a_Y * values[X2]);
		values[TY] = (a_Y * values[Y2]);
		values[TZ] = (a_Y * values[Z2]);*/

		values[TY] = a_Y;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::SetTranslationZ(float a_Z)
	{
		/*values[TX] = (a_Z * values[X3]);
		values[TY] = (a_Z * values[Y3]);
		values[TZ] = (a_Z * values[Z3]);*/

		values[TZ] = a_Z;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::SetTranslation(float a_X, float a_Y, float a_Z)
	{
		/*values[TX] = (a_X * values[X1]) + (a_Y * values[X2]) + (a_Z * values[X3]);
		values[TY] = (a_X * values[Y1]) + (a_Y * values[Y2]) + (a_Z * values[Y3]);
		values[TZ] = (a_X * values[Z1]) + (a_Y * values[Z2]) + (a_Z * values[Z3]);*/

		values[TX] = a_X;
		values[TY] = a_Y;
		values[TZ] = a_Z;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::SetTranslation(const Vec3& a_Translation)
	{
		/*values[TX] = (a_Translation.x * values[X1]) + (a_Translation.y * values[X2]) + (a_Translation.z * values[X3]);
		values[TY] = (a_Translation.x * values[Y1]) + (a_Translation.y * values[Y2]) + (a_Translation.z * values[Y3]);
		values[TZ] = (a_Translation.x * values[Z1]) + (a_Translation.y * values[Z2]) + (a_Translation.z * values[Z3]);*/

		values[TX] = a_Translation.x;
		values[TY] = a_Translation.y;
		values[TZ] = a_Translation.z;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::AddTranslationX(float a_X)
	{
		values[TX] += (a_X * values[X1]);
		values[TY] += (a_X * values[Y1]);
		values[TZ] += (a_X * values[Z1]);

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::AddTranslationY(float a_Y)
	{
		values[TX] += (a_Y * values[X2]);
		values[TY] += (a_Y * values[Y2]);
		values[TZ] += (a_Y * values[Z2]);

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::AddTranslationZ(float a_Z)
	{
		values[TX] += (a_Z * values[X3]);
		values[TY] += (a_Z * values[Y3]);
		values[TZ] += (a_Z * values[Z3]);

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::AddTranslation(float a_X, float a_Y, float a_Z)
	{
		values[TX] += (a_X * values[X1]) + (a_Y * values[X2]) + (a_Z * values[X3]);
		values[TY] += (a_X * values[Y1]) + (a_Y * values[Y2]) + (a_Z * values[Y3]);
		values[TZ] += (a_X * values[Z1]) + (a_Y * values[Z2]) + (a_Z * values[Z3]);

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::AddTranslation(const Vec3& a_Translation)
	{
		values[TX] += (a_Translation.x * values[X1]) + (a_Translation.y * values[X2]) + (a_Translation.z * values[X3]);
		values[TY] += (a_Translation.x * values[Y1]) + (a_Translation.y * values[Y2]) + (a_Translation.z * values[Y3]);
		values[TZ] += (a_Translation.x * values[Z1]) + (a_Translation.y * values[Z2]) + (a_Translation.z * values[Z3]);

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::FromLookAt(const Vec3& a_Position, const Vec3& a_Target, const Vec3& a_Up)
	{
		Vec3 f = (a_Target - a_Position).Normalize();
		Vec3 u = a_Up.GetNormalized();

		Vec3 s = f.GetCrossProduct(u).Normalize();
		u = s.GetCrossProduct(f);

		values[X1] = s.x;
		values[X2] = s.y;
		values[X3] = s.z;
		values[WX] = 0.f;

		values[Y1] = u.x;
		values[Y2] = u.y;
		values[Y3] = u.z;
		values[WY] = 0.f;

		values[Z1] = -f.x;
		values[Z2] = -f.y;
		values[Z3] = -f.z;
		values[WZ] = 0.f;

		values[TX] = -u.GetDotProduct(a_Position);
		values[TY] = -s.GetDotProduct(a_Position);
		values[TZ] = f.GetDotProduct(a_Position);
		values[CZ] = 1.f;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::FromAxisAngle(const Vec3& a_Axis, float a_Degrees)
	{
		/*
		void Mat4x4::BuildRotationMatrix( float x, float y, float z, float theta )
		{
		float cosT	= cos( theta );
		float sinT	= sin( theta );
		float T	 = 1.0f - cosT;

		_v[ 0] = T * x * x + cosT;
		_v[ 1] = T * x * y + sinT * z;
		_v[ 2] = T * x * z - sinT * y;
		_v[ 3] = 0;

		_v[ 4] = T * x * y - sinT * z;
		_v[ 5] = T * y * y + cosT;
		_v[ 6] = T * y * z + sinT * x;
		_v[ 7] = 0;

		_v[ 8] = T * x * z + sinT * y;
		_v[ 9] = T * y * z - sinT * x;
		_v[10] = T * z * z + cosT;
		_v[11] = 0;

		_v[12] = 0;
		_v[13] = 0;
		_v[14] = 0;
		_v[15] = 1; 
		}
		*/

		//Vec3 normal = a_Axis.GetNormalized();
		Vec3 normal = a_Axis;
		float sin_angle = Math::SinDeg(a_Degrees);
		float cos_angle = Math::CosDeg(a_Degrees);
		float inv_cos = 1.f - cos_angle;

		values[X1] = inv_cos * normal.x * normal.x + (cos_angle);
		values[Y1] = inv_cos * normal.x * normal.y + (sin_angle * normal.z);
		values[Z1] = inv_cos * normal.x * normal.z - (sin_angle * normal.y);
		values[WX] = 0.f;

		values[X2] = inv_cos * normal.x * normal.y - (sin_angle * normal.z);
		values[Y2] = inv_cos * normal.y * normal.y + (cos_angle);
		values[Z2] = inv_cos * normal.y * normal.z + (sin_angle * normal.x);
		values[WY] = 0.f;

		values[X3] = inv_cos * normal.x * normal.z + (sin_angle * normal.y);
		values[Y3] = inv_cos * normal.y * normal.z - (sin_angle * normal.x);
		values[Z3] = inv_cos * normal.z * normal.z + (cos_angle);
		values[WZ] = 0.f;

		/*float t = 1.f - cos_angle;
		values[SX] = cos_angle + a_Axis.x * a_Axis.x * t;
		values[SY] = cos_angle + a_Axis.y * a_Axis.y * t;
		values[SZ] = cos_angle + a_Axis.z * a_Axis.z * t;

		float tmp1 = a_Axis.x * a_Axis.y * t;
		float tmp2 = a_Axis.z * sin_angle;
		values[X2] = tmp1 + tmp2;
		values[Y1] = tmp1 - tmp2;

		tmp1 = a_Axis.x * a_Axis.z * t;
		tmp2 = a_Axis.y * sin_angle; 		
		values[X3] = tmp1 - tmp2;
		values[Z1] = tmp1 + tmp2;

		tmp1 = a_Axis.y * a_Axis.z * t;
		tmp2 = a_Axis.x * sin_angle;
		values[Y3] = tmp1 + tmp2;
		values[Z2] = tmp1 - tmp2;*/

		/*values[TX] = 0.f;
		values[TY] = 0.f;
		values[TZ] = 0.f;*/
		values[CZ] = 1.f;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::FromQuaternion(float a_X, float a_Y, float a_Z, float a_W)
	{
		float xn = a_X + a_X;
		float yn = a_Y + a_Y;
		float zn = a_Z + a_Z;

		const float xx = xn * a_X;
		const float xw = xn * a_W;

		const float yx = yn * a_X;
		const float yy = yn * a_Y;
		const float yw = yn * a_W;

		const float zx = zn * a_X;
		const float zy = zn * a_Y;
		const float zz = zn * a_Z;
		const float zw = zn * a_W;

		values[X1] = 1.f - (yy + zz);
		values[Y1] = yx - zw;
		values[Z1] = zx + yw;
		values[WX] = 0.f;

		values[X2] = yx + zw;
		values[Y2] = 1.f - (xx + zz);
		values[Z2] = zy - xw;
		values[WY] = 0.f;

		values[X3] = zx - yw;
		values[Y3] = zy + xw;
		values[Z3] = 1.f - (xx + yy);
		values[WZ] = 0.f;

		values[TX] = 0.f;
		values[TY] = 0.f;
		values[TZ] = 0.f;
		values[CZ] = 1.f;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::FromPerspective(float a_FOVy, float a_AspectRatio, float a_ZNear, float a_ZFar)
	{
		/*template <typename valType> 
		TB_INLINE detail::tmat4x4<valType> perspective(
			valType const & fovy, 
			valType const & aspect, 
			valType const & zNear, 
			valType const & zFar)
		{
			valType range = tan(radians(fovy / valType(2))) * zNear;	
			valType left = -range * aspect;
			valType right = range * aspect;
			valType bottom = -range;
			valType top = range;

			detail::tmat4x4<valType> Result(valType(0));
			Result[0][0] = (valType(2) * zNear) / (right - left);
			Result[1][1] = (valType(2) * zNear) / (top - bottom);
			Result[2][2] = - (zFar + zNear) / (zFar - zNear);
			Result[2][3] = - valType(1);
			Result[3][2] = - (valType(2) * zFar * zNear) / (zFar - zNear);
			return Result;
		}*/

		Clear();

		float range = Math::TanDeg(a_FOVy / 2.f) * a_ZNear;
		float width = range * a_AspectRatio * 2.f;
		float height = range * 2.f;

		values[SX] = (2.f * a_ZNear) / width;
		values[SY] = (2.f * a_ZNear) / height;
		values[SZ] = -((a_ZNear + a_ZFar) / (a_ZFar - a_ZNear));

		values[WZ] = -1.f;
		values[TZ] = -(2.f * a_ZNear * a_ZFar) / (a_ZFar - a_ZNear);

		return *this;
	}


	TB_INLINE Mat4x4& Mat4x4::FromFrustum(float a_Left, float a_Right, float a_Top, float a_Bottom, float a_ZNear, float a_ZFar)
	{
		// more thanks for GLM

		Clear();

		float width = a_Right - a_Left;
		float height = a_Bottom - a_Top;
		float zneardouble = 2.f * a_ZNear;
		float zdiff = a_ZFar - a_ZNear;

		values[SX] = zneardouble / width;
		values[SY] = zneardouble / height;
		values[SZ] = -(a_ZFar + a_ZNear) / zdiff;

		values[X3] = (a_Right + a_Left) / width;
		values[Y3] = (a_Top + a_Bottom) / height;
		values[WZ] = -1.f;
		values[TZ] = -(2.f * a_ZFar * a_ZNear) / zdiff;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::FromOrthographic(float a_Left, float a_Right, float a_Top, float a_Bottom, float a_ZNear, float a_ZFar)
	{
		// thanks go out to GLM

		Identity();

		values[SX] =  2.f / (a_Right - a_Left);
		values[SY] =  2.f / (a_Top - a_Bottom);
		values[SZ] = -2.f / (a_ZFar - a_ZNear);

		values[TX] = -(a_Right + a_Left) / (a_Right - a_Left);
		values[TY] = -(a_Top + a_Bottom) / (a_Top - a_Bottom);
		values[TZ] = -(a_ZFar + a_ZNear) / (a_ZFar - a_ZNear);

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::Lerp(const Mat4x4& a_Other, float a_Percentage)
	{
		values[X1] = values[X1] + ((a_Other.values[X1] - values[X1]) * a_Percentage);
		values[Y1] = values[Y1] + ((a_Other.values[Y1] - values[Y1]) * a_Percentage);
		values[Z1] = values[Z1] + ((a_Other.values[Z1] - values[Z1]) * a_Percentage);
		values[WX] = values[WX] + ((a_Other.values[WX] - values[WX]) * a_Percentage);

		values[X2] = values[X2] + ((a_Other.values[X2] - values[X2]) * a_Percentage);
		values[Y2] = values[Y2] + ((a_Other.values[Y2] - values[Y2]) * a_Percentage);
		values[Z2] = values[Z2] + ((a_Other.values[Z2] - values[Z2]) * a_Percentage);
		values[WY] = values[WY] + ((a_Other.values[WY] - values[WY]) * a_Percentage);

		values[X3] = values[X3] + ((a_Other.values[X3] - values[X3]) * a_Percentage);
		values[Y3] = values[Y3] + ((a_Other.values[Y3] - values[Y3]) * a_Percentage);
		values[Z3] = values[Z3] + ((a_Other.values[Z3] - values[Z3]) * a_Percentage);
		values[WZ] = values[WZ] + ((a_Other.values[WZ] - values[WZ]) * a_Percentage);

		values[TX] = values[TX] + ((a_Other.values[TX] - values[TX]) * a_Percentage);
		values[TY] = values[TY] + ((a_Other.values[TY] - values[TY]) * a_Percentage);
		values[TZ] = values[TZ] + ((a_Other.values[TZ] - values[TZ]) * a_Percentage);
		values[CZ] = values[CZ] + ((a_Other.values[CZ] - values[CZ]) * a_Percentage);

		return *this;
	}

	TB_INLINE Mat4x4 Mat4x4::GetLerp(const Mat4x4& a_Other, float a_Percentage)
	{
		Mat4x4 result;

		result.values[X1] = values[X1] + ((a_Other.values[X1] - values[X1]) * a_Percentage);
		result.values[Y1] = values[Y1] + ((a_Other.values[Y1] - values[Y1]) * a_Percentage);
		result.values[Z1] = values[Z1] + ((a_Other.values[Z1] - values[Z1]) * a_Percentage);
		result.values[WX] = values[WX] + ((a_Other.values[WX] - values[WX]) * a_Percentage);

		result.values[X2] = values[X2] + ((a_Other.values[X2] - values[X2]) * a_Percentage);
		result.values[Y2] = values[Y2] + ((a_Other.values[Y2] - values[Y2]) * a_Percentage);
		result.values[Z2] = values[Z2] + ((a_Other.values[Z2] - values[Z2]) * a_Percentage);
		result.values[WY] = values[WY] + ((a_Other.values[WY] - values[WY]) * a_Percentage);

		result.values[X3] = values[X3] + ((a_Other.values[X3] - values[X3]) * a_Percentage);
		result.values[Y3] = values[Y3] + ((a_Other.values[Y3] - values[Y3]) * a_Percentage);
		result.values[Z3] = values[Z3] + ((a_Other.values[Z3] - values[Z3]) * a_Percentage);
		result.values[WZ] = values[WZ] + ((a_Other.values[WZ] - values[WZ]) * a_Percentage);

		result.values[TX] = values[TX] + ((a_Other.values[TX] - values[TX]) * a_Percentage);
		result.values[TY] = values[TY] + ((a_Other.values[TY] - values[TY]) * a_Percentage);
		result.values[TZ] = values[TZ] + ((a_Other.values[TZ] - values[TZ]) * a_Percentage);
		result.values[CZ] = values[CZ] + ((a_Other.values[CZ] - values[CZ]) * a_Percentage);

		return result;
	}

	TB_INLINE Mat4x4& Mat4x4::AddRotationX(float a_Degrees)
	{
		float cos_rx = Math::CosDeg(a_Degrees);
		float sin_rx = Math::SinDeg(a_Degrees);

		/*
		float x1 = values[X1];
		float y1 = values[Y1];
		float z1 = values[Z1];
		float wx = values[WX];
		*/

		float x2 = values[X2];
		float y2 = values[Y2];
		float z2 = values[Z2];
		float wy = values[WY];

		float x3 = values[X3];
		float y3 = values[Y3];
		float z3 = values[Z3];
		float wz = values[WZ];

		values[X2] = /*(x1 * 0.f) + */(x2 * cos_rx) + (x3 * sin_rx);
		values[Y2] = /*(y1 * 0.f) + */(y2 * cos_rx) + (y3 * sin_rx);
		values[Z2] = /*(z1 * 0.f) + */(z2 * cos_rx) + (z3 * sin_rx);
		values[WY] = /*(wx * 0.f) + */(wy * cos_rx) + (wz * sin_rx);

		values[X3] = /*(x1 * 0.f) + */(x2 * -sin_rx) + (x3 * cos_rx);
		values[Y3] = /*(y1 * 0.f) + */(y2 * -sin_rx) + (y3 * cos_rx);
		values[Z3] = /*(z1 * 0.f) + */(z2 * -sin_rx) + (z3 * cos_rx);
		values[WZ] = /*(wx * 0.f) + */(wy * -sin_rx) + (wz * cos_rx);

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::SetRotationX(float a_Degrees)
	{
		Identity();
		return AddRotationX(a_Degrees);
	}

	TB_INLINE Mat4x4& Mat4x4::AddRotationY(float a_Degrees)
	{
		float cos_ry = Math::CosDeg(a_Degrees);
		float sin_ry = Math::SinDeg(a_Degrees);

		float x1 = values[X1];
		float y1 = values[Y1];
		float z1 = values[Z1];
		float wx = values[WX];

		/*
		float x2 = values[X2];
		float y2 = values[Y2];
		float z2 = values[Z2];
		float wy = values[WY];
		*/

		float x3 = values[X3];
		float y3 = values[Y3];
		float z3 = values[Z3];
		float wz = values[WZ];

		values[X1] = (x1 * cos_ry) + /*(x2 * 0.f) + */(x3 * -sin_ry);
		values[Y1] = (y1 * cos_ry) + /*(y2 * 0.f) + */(y3 * -sin_ry);
		values[Z1] = (z1 * cos_ry) + /*(z2 * 0.f) + */(z3 * -sin_ry);
		values[WX] = (wx * cos_ry) + /*(wy * 0.f) + */(wz * -sin_ry);

		values[X3] = (x1 * sin_ry) + /*(x2 * 0.f) + */(x3 * cos_ry);
		values[Y3] = (y1 * sin_ry) + /*(y2 * 0.f) + */(y3 * cos_ry);
		values[Z3] = (z1 * sin_ry) + /*(z2 * 0.f) + */(z3 * cos_ry);
		values[WZ] = (wx * sin_ry) + /*(wy * 0.f) + */(wz * cos_ry);

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::SetRotationY(float a_Degrees)
	{
		Identity();
		return AddRotationY(a_Degrees);
	}

	TB_INLINE Mat4x4& Mat4x4::AddRotationZ(float a_Degrees)
	{
		float cos_rz = Math::CosDeg(a_Degrees);
		float sin_rz = Math::SinDeg(a_Degrees);

		/*float radians1 = a_Degrees * Math::pi_radians;
		float radians2 = (a_Degrees * Math::pi) / 180.f;

		float cos1 = cosf(radians1);
		float cos2 = cosf(radians2);

		float sin1 = sinf(radians1);
		float sin2 = sinf(radians2);*/

		float x1 = values[X1];
		float y1 = values[Y1];
		float z1 = values[Z1];
		float wx = values[WX];

		float x2 = values[X2];
		float y2 = values[Y2];
		float z2 = values[Z2];
		float wy = values[WY];

		/*
		float x3 = values[X3];
		float y3 = values[Y3];
		float z3 = values[Z3];
		float wz = values[WZ];
		*/

		values[X1] = (x1 * cos_rz) + (x2 * sin_rz)/* + (x3 * 0.f)*/;
		values[Y1] = (y1 * cos_rz) + (y2 * sin_rz)/* + (y3 * 0.f)*/;
		values[Z1] = (z1 * cos_rz) + (z2 * sin_rz)/* + (z3 * 0.f)*/;
		values[WX] = (wx * cos_rz) + (wy * sin_rz)/* + (wz * 0.f)*/;

		values[X2] = (x1 * -sin_rz) + (x2 * cos_rz)/* + (x3 * 0.f)*/;
		values[Y2] = (y1 * -sin_rz) + (y2 * cos_rz)/* + (y3 * 0.f)*/;
		values[Z2] = (z1 * -sin_rz) + (z2 * cos_rz)/* + (z3 * 0.f)*/;
		values[WY] = (wx * -sin_rz) + (wy * cos_rz)/* + (wz * 0.f)*/;

		return *this;
	}

	TB_INLINE Mat4x4& Mat4x4::SetRotationZ(float a_Degrees)
	{
		Identity();
		return AddRotationZ(a_Degrees);
	}

	TB_INLINE Mat4x4& Mat4x4::Transpose()
	{
		Mat4x4 temp = *this;

		values[X1] = temp.values[X1];
		values[Y1] = temp.values[X2];
		values[Z1] = temp.values[X3];
		values[WX] = temp.values[TX];

		values[X2] = temp.values[Y1];
		values[Y2] = temp.values[Y2];
		values[Z2] = temp.values[Y3];
		values[WY] = temp.values[TY];

		values[X3] = temp.values[Z1];
		values[Y3] = temp.values[Z2];
		values[Z3] = temp.values[Z3];
		values[WZ] = temp.values[TZ];

		values[TX] = temp.values[WX];
		values[TY] = temp.values[WY];
		values[TZ] = temp.values[WZ];
		values[CZ] = temp.values[CZ];

		return *this;
	}

	TB_INLINE Mat4x4 Mat4x4::GetTransposed() const
	{
		Mat4x4 result = *this;

		result.values[X1] = values[X1];
		result.values[Y1] = values[X2];
		result.values[Z1] = values[X3];
		result.values[WX] = values[TX];

		result.values[X2] = values[Y1];
		result.values[Y2] = values[Y2];
		result.values[Z2] = values[Y3];
		result.values[WY] = values[TY];

		result.values[X3] = values[Z1];
		result.values[Y3] = values[Z2];
		result.values[Z3] = values[Z3];
		result.values[WZ] = values[TZ];

		result.values[TX] = values[WX];
		result.values[TY] = values[WY];
		result.values[TZ] = values[WZ];
		result.values[CZ] = values[CZ];

		return result;
	}

	TB_INLINE Mat4x4& Mat4x4::Invert()
	{
		Mat4x4 result;

		result.values[X1] = (values[Y2] * values[Z3]) - (values[Y3] * values[Z2]);
		result.values[Y1] = (values[Y3] * values[Z1]) - (values[Y1] * values[Z3]);
		result.values[Z1] = (values[Y1] * values[Z2]) - (values[Y2] * values[Z1]);
		result.values[WX] = 0;

		result.values[X2] = (values[X3] * values[Z2]) - (values[X2] * values[Z3]);
		result.values[Y2] = (values[X1] * values[Z3]) - (values[X3] * values[Z1]);
		result.values[Z2] = (values[X2] * values[Z1]) - (values[X1] * values[Z2]);
		result.values[WY] = 0;

		result.values[X3] = (values[X2] * values[Y3]) - (values[X3] * values[Y2]);
		result.values[Y3] = (values[X3] * values[Y1]) - (values[X1] * values[Y3]);
		result.values[Z3] = (values[X1] * values[Y2]) - (values[X2] * values[Y1]);
		result.values[WZ] = 0;

		result.values[TX] = 0.f;
		result.values[TY] = 0.f;
		result.values[TZ] = 0.f;
		result.values[CZ] = 1.f;

		float r = (values[X1] * result.values[X1]) + (values[Y1] * result.values[X2]) + (values[Z1] * result.values[X3]);
		float rcp_r = 1.f / r;
		float abs_r = Math::Abs(r);

		result.values[X1] *= rcp_r;
		result.values[Y1] *= rcp_r;
		result.values[Z1] *= rcp_r;

		result.values[X2] *= rcp_r;
		result.values[Y2] *= rcp_r;
		result.values[Z2] *= rcp_r;

		result.values[X3] *= rcp_r;
		result.values[Y3] *= rcp_r;
		result.values[Z3] *= rcp_r;

		result.values[TX] = (-values[TX] * result.values[X1]) + (-values[TY] * result.values[X2]) + (-values[TZ] * result.values[X3]);
		result.values[TY] = (-values[TX] * result.values[Y1]) + (-values[TY] * result.values[Y2]) + (-values[TZ] * result.values[Y3]);
		result.values[TZ] = (-values[TX] * result.values[Z1]) + (-values[TY] * result.values[Z2]) + (-values[TZ] * result.values[Z3]);

		*this = result;

		return *this;
	}

	TB_INLINE Mat4x4 Mat4x4::GetInverted() const
	{
		Mat4x4 result;

		result.values[X1] = (values[Y2] * values[Z3]) - (values[Y3] * values[Z2]);
		result.values[Y1] = (values[Y3] * values[Z1]) - (values[Y1] * values[Z3]);
		result.values[Z1] = (values[Y1] * values[Z2]) - (values[Y2] * values[Z1]);
		result.values[WX] = 0;

		result.values[X2] = (values[X3] * values[Z2]) - (values[X2] * values[Z3]);
		result.values[Y2] = (values[X1] * values[Z3]) - (values[X3] * values[Z1]);
		result.values[Z2] = (values[X2] * values[Z1]) - (values[X1] * values[Z2]);
		result.values[WY] = 0;

		result.values[X3] = (values[X2] * values[Y3]) - (values[X3] * values[Y2]);
		result.values[Y3] = (values[X3] * values[Y1]) - (values[X1] * values[Y3]);
		result.values[Z3] = (values[X1] * values[Y2]) - (values[X2] * values[Y1]);
		result.values[WZ] = 0;

		result.values[TX] = 0.f;
		result.values[TY] = 0.f;
		result.values[TZ] = 0.f;
		result.values[CZ] = 1.f;

		float r = (values[X1] * result.values[X1]) + (values[Y1] * result.values[X2]) + (values[Z1] * result.values[X3]);
		float rcp_r = 1.f / r;
		float abs_r = Math::Abs(r);

		result.values[X1] *= rcp_r;
		result.values[Y1] *= rcp_r;
		result.values[Z1] *= rcp_r;

		result.values[X2] *= rcp_r;
		result.values[Y2] *= rcp_r;
		result.values[Z2] *= rcp_r;

		result.values[X3] *= rcp_r;
		result.values[Y3] *= rcp_r;
		result.values[Z3] *= rcp_r;

		result.values[TX] = (-values[TX] * result.values[X1]) + (-values[TY] * result.values[X2]) + (-values[TZ] * result.values[X3]);
		result.values[TY] = (-values[TX] * result.values[Y1]) + (-values[TY] * result.values[Y2]) + (-values[TZ] * result.values[Y3]);
		result.values[TZ] = (-values[TX] * result.values[Z1]) + (-values[TY] * result.values[Z2]) + (-values[TZ] * result.values[Z3]);

		return result;
	}

	TB_INLINE Mat4x4& Mat4x4::InvertedTransposed()
	{
		// many thanks to GLM

		float sub00 = values[Z3] * values[CZ] - values[TZ] * values[WZ];
		float sub01 = values[Y3] * values[CZ] - values[TY] * values[WZ];
		float sub02 = values[Y3] * values[TZ] - values[TY] * values[Z3];
		float sub03 = values[X3] * values[CZ] - values[TX] * values[WZ];
		float sub04 = values[X3] * values[TZ] - values[TX] * values[Z3];
		float sub05 = values[X3] * values[TY] - values[TX] * values[Y3];
		float sub06 = values[Z2] * values[CZ] - values[TZ] * values[WY];
		float sub07 = values[Y2] * values[CZ] - values[TY] * values[WY];
		float sub08 = values[Y2] * values[TZ] - values[TY] * values[Z2];
		float sub09 = values[X2] * values[CZ] - values[TX] * values[WY];
		float sub10 = values[X2] * values[TZ] - values[TX] * values[Z2];
		float sub11 = values[Y2] * values[CZ] - values[TY] * values[WY];
		float sub12 = values[X2] * values[TY] - values[TX] * values[Y2];
		float sub13 = values[Z2] * values[WZ] - values[Z3] * values[WY];
		float sub14 = values[Y2] * values[WZ] - values[Y3] * values[WY];
		float sub15 = values[Y2] * values[Z3] - values[Y3] * values[Z2];
		float sub16 = values[X2] * values[WZ] - values[X3] * values[WY];
		float sub17 = values[X2] * values[Z3] - values[X3] * values[Z2];
		float sub18 = values[X2] * values[Y3] - values[X3] * values[Y2];

		Mat4x4 result;

		result.values[X1] = + (values[Y2] * sub00 - values[Z2] * sub01 + values[WY] * sub02);
		result.values[Y1] = - (values[X2] * sub00 - values[Z2] * sub03 + values[WY] * sub04);
		result.values[Z1] = + (values[X2] * sub01 - values[Y2] * sub03 + values[WY] * sub05);
		result.values[WX] = - (values[X2] * sub02 - values[Y2] * sub04 + values[Z2] * sub05);

		result.values[X2] = - (values[Y1] * sub00 - values[Z1] * sub01 + values[WX] * sub02);
		result.values[Y2] = + (values[X1] * sub00 - values[Z1] * sub03 + values[WX] * sub04);
		result.values[Z2] = - (values[X1] * sub01 - values[Y1] * sub03 + values[WX] * sub05);
		result.values[WY] = + (values[X1] * sub02 - values[Y1] * sub04 + values[Z1] * sub05);

		result.values[X3] = + (values[Y1] * sub06 - values[Z1] * sub07 + values[WX] * sub08);
		result.values[Y3] = - (values[X1] * sub06 - values[Z1] * sub09 + values[WX] * sub10);
		result.values[Z3] = + (values[X1] * sub11 - values[Y1] * sub09 + values[WX] * sub12);
		result.values[WZ] = - (values[X1] * sub08 - values[Y1] * sub10 + values[Z1] * sub12);

		result.values[TX] = - (values[Y1] * sub13 - values[Z1] * sub14 + values[WX] * sub15);
		result.values[TY] = + (values[X1] * sub13 - values[Z1] * sub16 + values[WX] * sub17);
		result.values[TZ] = - (values[X1] * sub14 - values[Y1] * sub16 + values[WX] * sub18);
		result.values[CZ] = + (values[X1] * sub15 - values[Y1] * sub17 + values[Z1] * sub18);

		float determinant = 
			(values[X1] * result.values[X1]) + 
			(values[Y1] * result.values[Y1]) + 
			(values[Z1] * result.values[Z1]) + 
			(values[WX] * result.values[WX]);

		result /= determinant;

		*this = result;

		return *this;
	}

	TB_INLINE Mat4x4 Mat4x4::GetInvertedTransposed() const
	{
		// many thanks to GLM

		float sub00 = values[Z3] * values[CZ] - values[TZ] * values[WZ];
		float sub01 = values[Y3] * values[CZ] - values[TY] * values[WZ];
		float sub02 = values[Y3] * values[TZ] - values[TY] * values[Z3];
		float sub03 = values[X3] * values[CZ] - values[TX] * values[WZ];
		float sub04 = values[X3] * values[TZ] - values[TX] * values[Z3];
		float sub05 = values[X3] * values[TY] - values[TX] * values[Y3];
		float sub06 = values[Z2] * values[CZ] - values[TZ] * values[WY];
		float sub07 = values[Y2] * values[CZ] - values[TY] * values[WY];
		float sub08 = values[Y2] * values[TZ] - values[TY] * values[Z2];
		float sub09 = values[X2] * values[CZ] - values[TX] * values[WY];
		float sub10 = values[X2] * values[TZ] - values[TX] * values[Z2];
		float sub11 = values[Y2] * values[CZ] - values[TY] * values[WY];
		float sub12 = values[X2] * values[TY] - values[TX] * values[Y2];
		float sub13 = values[Z2] * values[WZ] - values[Z3] * values[WY];
		float sub14 = values[Y2] * values[WZ] - values[Y3] * values[WY];
		float sub15 = values[Y2] * values[Z3] - values[Y3] * values[Z2];
		float sub16 = values[X2] * values[WZ] - values[X3] * values[WY];
		float sub17 = values[X2] * values[Z3] - values[X3] * values[Z2];
		float sub18 = values[X2] * values[Y3] - values[X3] * values[Y2];

		Mat4x4 result;

		result.values[X1] = + (values[Y2] * sub00 - values[Z2] * sub01 + values[WY] * sub02);
		result.values[Y1] = - (values[X2] * sub00 - values[Z2] * sub03 + values[WY] * sub04);
		result.values[Z1] = + (values[X2] * sub01 - values[Y2] * sub03 + values[WY] * sub05);
		result.values[WX] = - (values[X2] * sub02 - values[Y2] * sub04 + values[Z2] * sub05);

		result.values[X2] = - (values[Y1] * sub00 - values[Z1] * sub01 + values[WX] * sub02);
		result.values[Y2] = + (values[X1] * sub00 - values[Z1] * sub03 + values[WX] * sub04);
		result.values[Z2] = - (values[X1] * sub01 - values[Y1] * sub03 + values[WX] * sub05);
		result.values[WY] = + (values[X1] * sub02 - values[Y1] * sub04 + values[Z1] * sub05);

		result.values[X3] = + (values[Y1] * sub06 - values[Z1] * sub07 + values[WX] * sub08);
		result.values[Y3] = - (values[X1] * sub06 - values[Z1] * sub09 + values[WX] * sub10);
		result.values[Z3] = + (values[X1] * sub11 - values[Y1] * sub09 + values[WX] * sub12);
		result.values[WZ] = - (values[X1] * sub08 - values[Y1] * sub10 + values[Z1] * sub12);

		result.values[TX] = - (values[Y1] * sub13 - values[Z1] * sub14 + values[WX] * sub15);
		result.values[TY] = + (values[X1] * sub13 - values[Z1] * sub16 + values[WX] * sub17);
		result.values[TZ] = - (values[X1] * sub14 - values[Y1] * sub16 + values[WX] * sub18);
		result.values[CZ] = + (values[X1] * sub15 - values[Y1] * sub17 + values[Z1] * sub18);

		float determinant = 
			(values[X1] * result.values[X1]) + 
			(values[Y1] * result.values[Y1]) + 
			(values[Z1] * result.values[Z1]) + 
			(values[WX] * result.values[WX]);

		result /= determinant;

		return result;
	}

	TB_INLINE Vec3 Mat4x4::Transform(const Vec3& a_Vector) const
	{
		Vec3 result;

		float x = (a_Vector.x * values[X1]) + (a_Vector.y * values[X2]) + (a_Vector.z * values[X3]) + values[TX];
		float y = (a_Vector.x * values[Y1]) + (a_Vector.y * values[Y2]) + (a_Vector.z * values[Y3]) + values[TY];
		float z = (a_Vector.x * values[Z1]) + (a_Vector.y * values[Z2]) + (a_Vector.z * values[Z3]) + values[TZ];
		float w = (a_Vector.x * values[WX]) + (a_Vector.y * values[WY]) + (a_Vector.z * values[WZ]) + values[CZ];

		result.x = x / w;
		result.y = y / w;
		result.z = z / w;

		//result.x = x;
		//result.y = y;
		//result.z = z;

		return result;
	}

	TB_INLINE Vec3 Mat4x4::TransformRotation(const Vec3& a_Vector) const
	{
		Vec3 result;

		/*result.x = (a_Vector.x * values[X1]) + (a_Vector.y * values[Y1]) + (a_Vector.z * values[Z1]);
		result.y = (a_Vector.x * values[X2]) + (a_Vector.y * values[Y2]) + (a_Vector.z * values[Z2]);
		result.z = (a_Vector.x * values[X3]) + (a_Vector.y * values[Y3]) + (a_Vector.z * values[Z3]);*/

		result.x = (a_Vector.x * values[X1]) + (a_Vector.y * values[X2]) + (a_Vector.z * values[X3]);
		result.y = (a_Vector.x * values[Y1]) + (a_Vector.y * values[Y2]) + (a_Vector.z * values[Y3]);
		result.z = (a_Vector.x * values[Z1]) + (a_Vector.y * values[Z2]) + (a_Vector.z * values[Z3]);

		return result;
	}

	TB_INLINE Vec3 Mat4x4::TransformHomogenized(const Vec3& a_Vector) const
	{

	}

}